﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MathLib {

    public class DemogFilterAlg {

        private int count;
        private int groupCount;
        private Matrix[] p;
        private Matrix[] popMat;
        private double P0;
        private double vnu, ut;
        private Matrix A;
        public Matrix P;
        public Matrix G;

        public DemogFilterAlg(int count, int groupCount, Matrix[] p, Matrix[] popMat, double P0, double Vnu, double Ut)
        {
            init(count, groupCount, p, P0, Vnu, Ut);
        }

        public DemogFilterAlg(int count, int groupCount, Matrix[] p, Matrix A, double P0, double Vnu, double Ut)
        {
            if (count > p.Length)
                throw new Exception("Количество прогнозов превышает допустимое значение");
            
            init(count, groupCount, p, P0, Vnu, Ut);

            popMat = new Matrix[p.Length];
            for (int i = 0; i < popMat.Length; i++)
                popMat[i] = A;
        }

        private void init(int count, int groupCount, Matrix[] p, double P0, double Vnu, double Ut) {
            this.count = count;
            this.groupCount = groupCount;
            this.p = p;
            this.P0 = P0;
            this.vnu = Vnu;
            this.ut = Ut;
        }

        private Matrix GenerateC() {
            Matrix C = new Matrix(groupCount, groupCount);
            for (int i = 0; i < groupCount; i++)
                for (int j = 0; j < groupCount; j++)
                    C[i, j] = i == j ? 1.0 : 0.0;
            return C;
        }


       private Matrix PopAvg() {
            Matrix avg = new Matrix(groupCount, 1);
            for (int j = 0; j < p[0].NoRows; j++)
            {
                for (int i = 0; i < 15; i++)
                    avg[j, 0] = p[i][j, 0] + avg[j, 0];
                avg[j, 0] = avg[j, 0]/15;
            }
            return avg;
        }

        private Matrix GenerateNu()
        {
            Gauss g = new Gauss();
            Matrix m = new Matrix(groupCount, 1);
            for (int i = 0; i < m.NoRows; i++ )
                m[i, 0] = g.Next(5, Math.Sqrt(vnu));
                return m;
        }

        private Matrix GenerateP() {
            Gauss g = new Gauss();
            Matrix m = new Matrix(groupCount, groupCount);
            for (int i = 0; i < m.NoRows; i++)
                for (int j = 0; j < m.NoRows; j++)
                    if (i == j) m[i, j] = g.Next(P0, 1);
                    else m[i, j] = 0;
            return m;
        }


        private Matrix GenerateVnu() {
            Gauss g = new Gauss();
            Matrix m = new Matrix(groupCount, groupCount);
            for (int i = 0; i < m.NoRows; i++)
                for (int j = 0; j < m.NoRows; j++)
                    if (i == j) m[i, j] = g.Next(vnu, 0.0005);
                    else m[i, j] = 0;
            return m;
        }

        private Matrix GenerateUt() {
            Gauss g = new Gauss();
            Matrix m = new Matrix(groupCount, groupCount);
            for (int i = 0; i < m.NoRows; i++)
                for (int j = 0; j < m.NoRows; j++)
                    if (i == j) m[i, j] = g.Next(ut, 0.0005);
                    else m[i, j] = 0;
            return m;
        }

        private Matrix GenerateU() {
            Matrix avg = PopAvg();
            Gauss g = new Gauss();
            Matrix m = new Matrix(groupCount, 1);
            for (int i = 0; i < m.NoRows; i++)
                m[i, 0] = g.Next(2, Math.Sqrt(ut));
            return m;

              }


        public Matrix SolveWithFilter() {

            Matrix C = this.GenerateC();
            P = GenerateP();
            Matrix Nu = GenerateNu();
            Matrix Ut = GenerateUt();
            Matrix Vnu = GenerateVnu();
            PopulationModeling pm = new PopulationModeling(groupCount);
            Matrix U = pm.GenerateMigrationRate();
            Matrix x = p[0];
            for (int i = 0; i < count; i++) {
              Matrix G = Matrix.Transpose(C) * P * C + Vnu;
                G = Matrix.Inverse(G);
               Matrix A = pm.GenerateA();
              // Matrix A = popMat[i];
                G = A * P * C * G;
                P = A * P * Matrix.Transpose(A) + Ut - G * Matrix.Transpose(C) * P * Matrix.Transpose(A);
               Matrix y = C * p[i] + Nu; //зашумл данные
              //  y = p[i];
 
                x = A * x + U + G * (y - C * x); //  MNu = 0
        
            }
            return x;

        }

        public Matrix Solve() {
            Matrix y = null;
            PopulationModeling pm = new PopulationModeling(groupCount);

            Matrix x = p[0];
            for (int i = 0; i < count; i++) {
                Matrix A = pm.GenerateA();
               // Matrix A = popMat[i];
                Matrix U = pm.GenerateMigrationRate();
                Matrix C = GenerateC();
                Matrix Nu = GenerateNu();
                y = A * x + U;
                x =  C * y + Nu; //зашумл данные
               //x = y;
            }
            return x;
        }

        public Matrix SolveReal() {
            Matrix y = new Matrix(groupCount, 1);
          

            Matrix x = p[0];
            for (int i = 0; i < count; i++) {

                Matrix U = GenerateU();
                Matrix C = GenerateC();
                Matrix Nu = GenerateNu();
            
                y = A * x + U;
                x = C * y + Nu;//зашумл данные
               // x = y; 
            }
            return x;
        }

        public Matrix SolveWithFilterReal() {

             G = new Matrix(groupCount, groupCount);
            Matrix C = this.GenerateC();
            P = GenerateP();
           
            Matrix Ut = GenerateUt();
           Matrix Vnu = GenerateVnu();
          //  Matrix Vnu = Matrix.Zero(groupCount);
            PopulationModeling pm = new PopulationModeling(groupCount);
           // Matrix U = pm.GenerateMigrationRate();
            Matrix U = GenerateU();
           // Matrix U = Matrix.Zero(groupCount);
            Matrix x = p[0];
            for (int i = 0; i < count; i++) {
                Matrix Nu = GenerateNu();
                G = Matrix.Transpose(C) * P * C + Vnu;
                G = Matrix.Inverse(G);
                G = A * P * C * G;
                Matrix y = C * p[i] + Nu;
                x = A * x + U + G * (y - C * x);//мат ожидание нулевое
                P = A * P * Matrix.Transpose(A) + Ut - G * Matrix.Transpose(C) * P * Matrix.Transpose(A);
            }

            return x;

        }

        public static Matrix CalclDeviation(Matrix m, Matrix[] estimate) {
            int minRow = estimate.Min(a => a.NoRows);
            int maxRow = estimate.Max(a => a.NoRows);
            if (minRow != maxRow)
                throw new Exception("Размерности векторов-оценок не совпадают");

            int rowCount = minRow;
            if (rowCount != m.NoRows)
                throw new Exception("Неверная размерность вектора мат. ожиданий");

            Matrix dev = new Matrix(rowCount, 1); ;
            for (int i = 0; i < rowCount; i++) {
                double d = 0;
                for (int j = 0; j < estimate.Length; j++) {

                    double s = m[i, 0] - estimate[j][i, 0];
                    d += s * s;
                }

                dev[i, 0] = Math.Sqrt(d / estimate.Length)/m[i,0];
            }

            return dev;
        }
    }
}